<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.13"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>MRSL Motion Primitive Library: MRSL Motion Primitive Library for quadrotor v1.2</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">MRSL Motion Primitive Library
   &#160;<span id="projectnumber">1.2</span>
   </div>
   <div id="projectbrief">A motion primitive library for generating trajectory for mobile robots</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.13 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
$(function() {
  initMenu('',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
</script>
<div id="main-nav"></div>
</div><!-- top -->
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">MRSL Motion <a class="el" href="classPrimitive.html" title="Primitive class. ">Primitive</a> Library for quadrotor v1.2 </div>  </div>
</div><!--header-->
<div class="contents">
<div class="textblock"><p><a href="https://app.wercker.com/project/byKey/ae575e9b5218e1f02684065445c6de66"></a> </p><hr/>
<p> Motion <a class="el" href="classPrimitive.html" title="Primitive class. ">Primitive</a> Library is a search-based planner to compute dynamically feasible trajectories for a quadrotor flying in an obstacle-cluttered environment. Our approach searches for smooth, minimum-time trajectories by exploring the map using a set of short-duration motion primitives. The primitives are generated by solving an optimal control problem and induce a finite lattice discretization on the state space which can be explored using a graph-search algorithm. The proposed approach is able to generate resolution-complete (i.e., optimal in the discretized space), safe, dynamically feasibility trajectories efficiently by exploiting the explicit solution of a Linear Quadratic Minimum Time problem. It does not assume a hovering initial condition and, hence, is suitable for fast online re-planning while the robot is moving.</p>
<p>More details about the algorithm can be found in following publications:</p><ul>
<li>S. Liu, N. Atanasov, K. Mohta, and V. Kumar, <a href="http://ieeexplore.ieee.org/document/8206119/">"Search-based Motion Planning for Quadrotors using Linear Quadratic Minimum Time Control"</a>, IROS 2017</li>
<li>S. Liu, K. Mohta, N. Atanasov, and V. Kumar, <a href="https://arxiv.org/abs/1810.03071">"Towards Search-based Motion Planning for Micro Aerial Vehicles"</a>, arxiv 2018</li>
</ul>
<h2>New Features in v1.2</h2>
<ul>
<li>Add iterative plan in MapPlanner</li>
<li>Remove dependence on SDL library, using OpenCV for plotting</li>
</ul>
<h2>New Features in v1.1</h2>
<ul>
<li>Reformat the repo structure</li>
<li>Add yaw primitive</li>
<li>Add potential function to perturb trajectory</li>
<li>Separate <code>ellipsoid_planner</code> to <a href="https://github.com/sikang/mpl_ros.git"><code>mpl_ros</code></a></li>
</ul>
<h2>Installation</h2>
<h4>Prerequisite:</h4>
<ul>
<li><code>Eigen</code>: apt install libeigen3-dev</li>
<li><code>YAML-CPP</code>: apt install libyaml-cpp-dev</li>
<li><code>OpenCV</code>: apt install libopencv-dev</li>
</ul>
<p>or simply run following commands: </p><div class="fragment"><div class="line">$ sudo apt-get update</div><div class="line">$ sudo apt install -y libeigen3-dev libyaml-cpp-dev libproj-dev libopencv-dev cmake</div></div><!-- fragment --><p>#### A) Simple cmake </p><div class="fragment"><div class="line">mkdir build &amp;&amp; cd build &amp;&amp; cmake .. &amp;&amp; make -j4</div></div><!-- fragment --><p>#### B) Using Catkin (not recognizable by catkin_make) </p><div class="fragment"><div class="line">$ mv motion_primitive_library ~/catkin_ws/src</div><div class="line">$ cd ~/catkin_ws &amp; catkin_make_isolated -DCMAKE_BUILD_TYPE=Release</div></div><!-- fragment --><h4>CTest</h4>
<p>Run following command in the <code>build</code> folder for testing the executables: </p><div class="fragment"><div class="line">$ make test</div></div><!-- fragment --><p>If everything works, you should see the results as: </p><div class="fragment"><div class="line">Total Test time (real) =   4.22 sec</div><div class="line">Running tests...</div><div class="line">Test project /home/sikang/thesis_ws/src/packages/mpl_ros/motion_primitive_library/build</div><div class="line">    Start 1: test_traj_solver</div><div class="line">1/6 Test #1: test_traj_solver ........................   Passed    0.00 sec</div><div class="line">    Start 2: test_planner_2d</div><div class="line">2/6 Test #2: test_planner_2d .........................   Passed    0.92 sec</div><div class="line">    Start 3: test_planner_2d_prior_traj</div><div class="line">3/6 Test #3: test_planner_2d_prior_traj ..............   Passed    0.93 sec</div><div class="line">    Start 4: test_planner_2d_with_yaw</div><div class="line">4/6 Test #4: test_planner_2d_with_yaw ................   Passed    0.96 sec</div><div class="line">    Start 5: test_distance_map_planner_2d</div><div class="line">5/6 Test #5: test_distance_map_planner_2d ............   Passed    1.33 sec</div><div class="line">    Start 6: test_distance_map_planner_2d_with_yaw</div><div class="line">6/6 Test #6: test_distance_map_planner_2d_with_yaw ...   Passed    2.39 sec</div><div class="line"></div><div class="line">100% tests passed, 0 tests failed out of 6</div><div class="line"></div><div class="line">Total Test time (real) =   6.54 sec</div></div><!-- fragment --><h4>Include in other projects:</h4>
<p>To link this lib properly, add following in the <code>CMakeLists.txt</code> </p><div class="fragment"><div class="line">find_package(motion_primitive_library REQUIRED)</div><div class="line">include_directories(${MOTION_PRIMITIVE_LIBRARY_INCLUDE_DIRS})</div><div class="line">...</div><div class="line">add_executable(test_xxx src/test_xxx.cpp)</div><div class="line">target_link_libraries(test_xxx ${MOTION_PRIMITIVE_LIBRARY_LIBRARIES})</div></div><!-- fragment --><h2>Example Usage</h2>
<h3>Preparation</h3>
<p>To run the planner, three components are required to be set properly:</p>
<h4>1) Set Start and Goal:</h4>
<p>We use the<code>class <a class="el" href="structWaypoint.html" title="Waypoint base class. ">Waypoint</a></code> for the start and goal. A <code><a class="el" href="structWaypoint.html" title="Waypoint base class. ">Waypoint</a></code> contains coordinates of position, velocity, etc and the flag <code>use_xxx</code> to indicate the control input. An example for 2D planning is given as: </p><div class="fragment"><div class="line">{c++}</div><div class="line">Waypoint2D start, goal; // Initialize start and goal as Waypoint2D</div><div class="line">start.pos = Vec3f(2.5, -3.5);</div><div class="line">start.use_pos = true;</div><div class="line">start.use_vel = true;</div><div class="line">start.use_acc = false;</div><div class="line">start.use_jrk = false;</div><div class="line">start.use_yaw = false;</div><div class="line">goal.pos = Vec3f(35, 2.5);</div><div class="line">goal.control = start.control;</div></div><!-- fragment --><p>The flag <code>use_xxx</code> indicates the planner to plan in different control space. For example, the above example code sets the control in <code>ACC</code> space. Eight options are provided by setting following flags:</p>
<table class="doxtable">
<tr>
<th align="left">~ </th><th align="left">VEL </th><th align="left">ACC </th><th align="left">JRK </th><th align="left">SNP </th><th align="left">VEL&amp;YAW </th><th align="left">ACC&amp;YAW </th><th align="left">JRK&amp;YAW </th><th align="left">SNP&amp;YAW  </th></tr>
<tr>
<td align="left"><code>use_pos:</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td></tr>
<tr>
<td align="left"><code>use_vel:</code> </td><td align="left"><code>false</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>false</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td></tr>
<tr>
<td align="left"><code>use_acc:</code> </td><td align="left"><code>false</code> </td><td align="left"><code>false</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>false</code> </td><td align="left"><code>false</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td></tr>
<tr>
<td align="left"><code>use_jrk:</code> </td><td align="left"><code>false</code> </td><td align="left"><code>false</code> </td><td align="left"><code>false</code> </td><td align="left"><code>true</code> </td><td align="left"><code>false</code> </td><td align="left"><code>false</code> </td><td align="left"><code>false</code> </td><td align="left"><code>true</code> </td></tr>
<tr>
<td align="left"><code>use_yaw:</code> </td><td align="left"><code>false</code> </td><td align="left"><code>false</code> </td><td align="left"><code>false</code> </td><td align="left"><code>false</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td><td align="left"><code>true</code> </td></tr>
</table>
<p>In equal, one can also set the attribute <code>control</code> of <code><a class="el" href="structWaypoint.html" title="Waypoint base class. ">Waypoint</a></code> for the same purpose:</p>
<table class="doxtable">
<tr>
<th align="left">~ </th><th align="left">VEL </th><th align="left">ACC </th><th align="left">JRK </th><th align="left">SNP </th><th align="left">VEL&amp;YAW </th><th align="left">ACC&amp;YAW </th><th align="left">JRK&amp;YAW </th><th align="left">SNP&amp;YAW  </th></tr>
<tr>
<td align="left"><code>control:</code> </td><td align="left"><code>Control::VEL</code> </td><td align="left"><code>Control::ACC</code> </td><td align="left"><code>Control::JRK</code> </td><td align="left"><code>Control::SNP</code> </td><td align="left"><code>Control::VELxYAW</code> </td><td align="left"><code>Control::ACCxYAW</code> </td><td align="left"><code>Control::JRKxYAW</code> </td><td align="left"><code>Control::SNPxYAW</code> </td></tr>
</table>
<h4>2) Set collision checking method:</h4>
<p>Any planner needs a collision checking function, there are several utils in this package to checking collision for obstacles in different representations. In the most common environment where obstacles are represented as voxels, we use <code>class MapUtil</code> which is a template class that adapts to 2D (<code>OccMapUtil</code>) and 3D (<code>VoxelMapUtil</code>). An example for initializing a 2D collision checking <code>OccMapUtil</code> is given as: </p><div class="fragment"><div class="line">{c++}</div><div class="line">std::shared_ptr&lt;MPL::OccMapUtil&gt; map_util; // Declare as a shared pointer</div><div class="line">map_util.reset(new MPL::OccMapUtil); // Initialize map_util</div><div class="line">map_util-&gt;setMap(origin, dim, data, resolution); // Set the map information</div><div class="line">...</div><div class="line">planner-&gt;setMapUtil(map_util); // Set collision checking util</div></div><!-- fragment --><p>Here <code>origin</code>, <code>dim</code>, <code>data</code> and <code>resolution</code> are user input.</p>
<h4>3) Set control input:</h4>
<p>Our planner takes control input to generate primitives. User need to specify it before start planning. An example for the control input <code>U</code> for 2D planning is given as following, in this case, <code>U</code> simply include 9 elements: </p><div class="fragment"><div class="line">{c++}</div><div class="line">decimal_t u_max = 0.5;</div><div class="line">vec_E&lt;VecDf&gt; U;</div><div class="line">const decimal_t du = u_max / num;</div><div class="line">for(decimal_t dx = -u_max; dx &lt;= u_max; dx += du )</div><div class="line">  for(decimal_t dy = -u_max; dy &lt;= u_max; dy += du )</div><div class="line">      U.push_back(Vec2f(dx, dy));</div><div class="line">...</div><div class="line">planner-&gt;setU(U); // Set control input</div></div><!-- fragment --><h3>Run the planner:</h3>
<p>After setting up above 3 required components, a plan thread can be launched as: </p><div class="fragment"><div class="line">{c++}</div><div class="line">std::unique_ptr&lt;MPL::OccMapPlanner&gt; planner(new MPL::OccMapPlanner(true)); // Declare a 2D planner with verbose</div><div class="line">planner-&gt;setMapUtil(map_util); // Set collision checking util</div><div class="line">planner-&gt;setU(U); // Set control input</div><div class="line">planner-&gt;setDt(1.0); // Set dt for each primitive</div><div class="line"></div><div class="line">bool valid = planner-&gt;plan(start, goal); // Plan from start to goal</div></div><!-- fragment --><h2>Test Examples</h2>
<h4>Example1 (direct plan):</h4>
<p>After compiling by <code>cmake</code>, run following command for test a 2D planning in a given map: </p><div class="fragment"><div class="line">$ ./build/test_planner_2d ./data/corridor.yaml</div></div><!-- fragment --><p>You should see following messages if it works properly: </p><div class="fragment"><div class="line">[MapPlanner] PLANNER VERBOSE ON</div><div class="line">[PlannerBase] set v_max: 1.000000</div><div class="line">[PlannerBase] set a_max: 1.000000</div><div class="line">[PlannerBase] set dt: 1.000000</div><div class="line">...</div><div class="line">++++++++++++++++++ PLANNER +++++++++++++++</div><div class="line">+                  w: 10.00               +</div><div class="line">+                 dt: 1.00               +</div><div class="line">+                 ds: 0.0100               +</div><div class="line">+                 dv: 0.1000               +</div><div class="line">+                 da: 0.1000               +</div><div class="line">+                 dj: 0.1000               +</div><div class="line">+              v_max: 1.00               +</div><div class="line">+              a_max: 1.00               +</div><div class="line">+              j_max: -1.00               +</div><div class="line">+              U num: 9                +</div><div class="line">+            tol_dis: 0.50               +</div><div class="line">+            tol_vel: 0.00               +</div><div class="line">+            tol_acc: 0.00               +</div><div class="line">+              alpha: 0                 +</div><div class="line">+heur_ignore_dynamics: 1                 +</div><div class="line">++++++++++ PLANNER +++++++++++</div><div class="line"></div><div class="line">Start from new node!</div><div class="line">goalNode fval: 358.000000, g: 353.000000!</div><div class="line">Expand [291] nodes!</div><div class="line">...</div><div class="line">Reached Goal !!!!!!</div><div class="line">MPL Planner takes: 9.000000 ms</div><div class="line">MPL Planner expanded states: 615</div><div class="line">Total time T: 35.000000</div><div class="line">Total J:  J(VEL) = 36.750000, J(ACC) = 1.500000, J(JRK) = 0.000000, J(SNP) = 0.000000</div></div><!-- fragment --><p>The output image is saved in the current folder: </p><div class="image">
<img src="test_planner_2d.jpg" alt="test_planner_2d.jpg"/>
<div class="caption">
Visualization</div></div>
<p>(blue dots show the expended states, blue and cyan circles indicate start and goal).</p>
<h4>Example2 (plan with a prior trajectory):</h4>
<p>Run following command for test a 2D planning, it first finds a trajector in low dimensional space (acceleration-driven), then it uses the planned trajectory to refine for a trajectory in high dimensional space (jerk-driven): </p><div class="fragment"><div class="line">$ ./build/test_planner_2d_prior_traj ./data/corridor.yaml</div></div><!-- fragment --><p>In the following output image, the black curve is the prior trajectory: </p><div class="image">
<img src="./data/test_planner_2d_prior_traj.jpg" alt="Visualization"/>
</div>
<h4>Example3 (plan with yaw constraint):</h4>
<p>In some cases, the robot needs to move forward within the FOV of the camera or range sensor such that the yaw needs to be considered when planning. <code>MapPlanner</code> handles the yaw constraint properly. Following image shows the output of running: </p><div class="fragment"><div class="line">$ ./build/test_planner_2d_with_yaw ./data/corridor.yaml</div></div><!-- fragment --> <div class="image">
<img src="test_planner_2d_with_yaw.jpg" alt="test_planner_2d_with_yaw.jpg"/>
<div class="caption">
Visualization</div></div>
 <h4>Example4 (perturb trajectory with potential field):</h4>
<p>In practical case, the robot wants to stay away from obstacles even though the nominal trajectory is collision free. To add a soft constraint based on the distance towards obstacles, one technique is to use the artificial potential field (APF). In this examplem, we show how to perturb a nominal trajectory based on the search-based method with APFs: </p><div class="fragment"><div class="line">$ ./build/test_distance_map_planner_2d ./data/corridor.yaml</div></div><!-- fragment --> <div class="image">
<img src="test_distance_map_planner_2d.jpg" alt="test_distance_map_planner_2d.jpg"/>
<div class="caption">
Visualization</div></div>
<p> In addition, to do the perturbation iteratively, run the other node: </p><div class="fragment"><div class="line">$ ./build/test_distance_map_planner_2d_iterative ./data/corridor.yaml</div></div><!-- fragment --> <div class="image">
<img src="test_distance_map_planner_2d_iterative.jpg" alt="test_distance_map_planner_2d_iterative.jpg"/>
<div class="caption">
Visualization</div></div>
<h4>Example5 (perturb trajectory with potential field and yaw constraint):</h4>
<p>In a more comprehensive case, when the robot has limited FOV and sensing range, plan the trajectory that considers safety and yaw constraint: </p><div class="fragment"><div class="line">$ ./build/test_distance_map_planner_2d_with_yaw ./data/corridor.yaml</div></div><!-- fragment --><div class="image">
<img src="test_distance_map_planner_2d_with_yaw.jpg" alt="test_distance_map_planner_2d_with_yaw.jpg"/>
<div class="caption">
Visualization</div></div>
 <h4>Example6 (trajectory generation):</h4>
<p>This example illustrate the <code>mpl_traj_solver</code> which is a smoothing tool to derive a smoother trajectory. An example of generating trajectory from a given path, without obstacles: </p><div class="fragment"><div class="line">$ ./build/test_traj_solver</div></div><!-- fragment --> <div class="image">
<img src="test_traj_solver.jpg" alt="test_traj_solver.jpg"/>
<div class="caption">
Visualization</div></div>
<p> Here we generate three different trajectories using the same path and time allocation: the red one is minimum velocity trajectory, the green one is the minimum acceleration trajectory and the blue one is the minimum jerk trajectory.</p>
<h2>Doxygen</h2>
<p>For API, please refer to <a href="https://sikang.github.io/motion_primitive_library">Doxygen</a>.</p>
<h2>ROS Wrapper</h2>
<p>The interface with ROS for visualization and implementation can be found in <a href="https://github.com/sikang/mpl_ros.git"><code>mpl_ros</code></a>. </p>
</div></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.13
</small></address>
</body>
</html>
